//
// Created by 000 on 2025/5/23.
//

#include "gy33_i2c.h"
#include "i2c.h"

float Linear3D_Centroid[4][3] = {
        {85.7908,64.8039,72.4183},        //Red
        {137.6047,145.7305,99.4850},        //Yellow
        {38.5448,80.9551,100.8525},        //Blue
        {75.5818,123.7757,89.8303}        //Green
};

float Linear3D_Direction[4][3] = {
        {0.7661,0.4518,0.4569},       //Red
        {0.8245,0.4801,0.2992},       //Yellow
        {0.7405,0.4098,0.5325},       //Blue
        {0.6455,0.6919,0.3232}        //Green
};


void gy33_SendConfig(uint8_t *config)
{
    HAL_I2C_Mem_Write(&hi2c1, GY33_ADDR, GY33_CONFIG, I2C_MEMADD_SIZE_8BIT, config, 1, HAL_MAX_DELAY);
}

uint8_t gy33_GetByte(uint8_t mem_addr)
{
    uint8_t data = 0;
    HAL_I2C_Mem_Read(&hi2c1, GY33_ADDR, mem_addr, I2C_MEMADD_SIZE_8BIT, &data, 1, HAL_MAX_DELAY);
    return data;
}

void gy33_GetData(color_t *color)
{
    uint8_t bufferH, bufferL;

    bufferH = gy33_GetByte(GY33_RAW_RED_H);
    bufferL = gy33_GetByte(GY33_RAW_RED_L);
    color->raw_R = (bufferH << 8) | bufferL;

    bufferH = gy33_GetByte(GY33_RAW_GREEN_H);
    bufferL = gy33_GetByte(GY33_RAW_GREEN_L);
    color->raw_G = (bufferH << 8) | bufferL;

    bufferH = gy33_GetByte(GY33_RAW_BLUE_H);
    bufferL = gy33_GetByte(GY33_RAW_BLUE_L);
    color->raw_B = (bufferH << 8) | bufferL;

    bufferH = gy33_GetByte(GY33_RAW_CLEAR_H);
    bufferL = gy33_GetByte(GY33_RAW_CLEAR_L);
    color->raw_C = (bufferH << 8) | bufferL;

    bufferH = gy33_GetByte(GY33_LUX_H);
    bufferL = gy33_GetByte(GY33_LUX_L);
    color->Lux = (bufferH << 8) | bufferL;

    bufferH = gy33_GetByte(GY33_CT_H);
    bufferL = gy33_GetByte(GY33_CT_L);
    color->CT = (bufferH << 8) | bufferL;

    color->R = gy33_GetByte(GY33_R);
    color->G = gy33_GetByte(GY33_G);
    color->B = gy33_GetByte(GY33_B);

    color->color = gy33_GetByte(GY33_COLOR);
}

//float CalDistance(color_t *color,uint8_t RYBG){
//    float vector[3];
//    float Vertical[3];
//    vector[0] = 1.0 * color->R - Linear3D_Centroid[RYBG][0];
//    vector[1] = 1.0 * color->G - Linear3D_Centroid[RYBG][1];
//    vector[2] = 1.0 * color->B - Linear3D_Centroid[RYBG][2];
//
//    Vertical[0] = vector[1]*Linear3D_Direction[RYBG][2] - vector[2] * Linear3D_Direction[RYBG][1];
//    Vertical[1] = vector[2]*Linear3D_Direction[RYBG][0] - vector[0] * Linear3D_Direction[RYBG][2];
//    Vertical[2] = vector[0]*Linear3D_Direction[RYBG][1] - vector[1] * Linear3D_Direction[RYBG][0];
//
//    float distant = Vertical[0]*Vertical[0] + Vertical[1]*Vertical[1] + Vertical[2]*Vertical[2];
//
//    return distant;
//}

//int8_t gy33_GetColor(color_t *average)
//{
//    RGB_t res = Red;
//    float min = 15000.0;
//
//    for(uint8_t i=0;i<3;i++){
//        float distant = CalDistance(average, i);
//        if(distant < min) {
//            min = distant;
//            res = i;
//        }
//        usart_printf("info: %d, %.4f\n",
//                     i, distant);
//        HAL_Delay(500);
//    }
////    usart_printf("%d\t%.4f\n",res,min);
//    return res;
//}

int8_t gy33_ColorIdentify()
{
    color_t raw;
    gy33_GetData(&raw);

    RGB_t result = Default;

    if(raw.R >= raw.G && raw.B >= raw.G)result = Red;
    else if(raw.B >= raw.G && raw.G >= raw.R)result = Blue;
    else if(raw.G >= raw.B && raw.B >= raw.R)result = Green;
    else if(raw.G >= raw.B && raw.R >= raw.B)result = Yellow;

    usart_printf("%d, %d, %d\n", raw.R, raw.G, raw.B);

    return result;
}

